/**
 * \file
 * \date 19.09.2013
 * \author alex
 */
#include <bullet_rviz_test/physical_object.h>

namespace bullet_rviz_test
{

PhysicalObject::PhysicalObject()
{
}

PhysicalObject::~PhysicalObject()
{
}

void PhysicalObject::registerMotionCallback(MotionCallback motion_callback)
{
  motion_callbacks_.push_back(motion_callback);
}

void PhysicalObject::notifyChangeCallbacks()
{
  for (MotionCallbackVector::iterator it = motion_callbacks_.begin();
      it != motion_callbacks_.end(); ++it)
    (*it)(*this);
}

PhysicalObject::MotionState::MotionState(PhysicalObject * physical_object,
    const btTransform & world_transform) :
    physical_object_(physical_object), world_transform_(world_transform)
{
}

PhysicalObject::MotionState::~MotionState()
{
}

void PhysicalObject::MotionState::getWorldTransform(
    btTransform & world_transform) const
{
  world_transform = world_transform_;
}

void PhysicalObject::MotionState::setWorldTransform(
    const btTransform & world_transform)
{
  world_transform_ = world_transform;
  physical_object_->notifyChangeCallbacks();
}

}
